﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Microsoft.Devices.Sensors;
using Microsoft.Xna.Framework;
using System.Threading;
using StaticView.Input;
using S = StaticView.Algorithm;
using System.Diagnostics;

namespace StaticView
{
    // Static Position class
    //      Contain Postion thread process
    //      Store the position of the phone
    static class Position
    {
        public static Vector3 Normal { get; private set; } // Normal vector (unit) of the plane (local z)
        public static Vector3 LocalX { get; private set; } // Local x vector (unit) of the phone
        public static Vector3 Coordinates { get; private set; } // Coordinates of the phone in 3D

        static List<ISensor> presensors; // List of sensors which are not calibrated yet
        static List<ISensor> sensors; // List of calibrated sensors
        static volatile bool stop = false;
        static bool AccurateMode = false; // Accurate Mode: start when if there're at least 1 gyroscope

        // Vectors
        static Vector3 gravity;
        static Vector3 magnetic;

        // Quaternion (which also the states of the system)
        static Quaternion Q = new Quaternion(0, 0, 0, 1);

        // Stopwatch
        static Stopwatch Watch;
        static long nanosPerTick;
        static List<UpdatePoint> UpdateList;

        // Extended Kalman Filter instance
        static S.ExtendedKalmanFilter EKL;
        // EKF constants & variables
        const int NOSTATES = 3;
        const int NOCONTROLS = 3;
        const float LOWPASSNOISE = 5;

        // Acceleration Separator instance
        static S.AccelerationSeparator AS;
        static ISensor ASAccelerometer, ASMagnetometer;
        static TimeSpan ASTime;

        // Calibration variables
        static int noacce = 0; // Number of calibrated accelerometer
        static int nogyro = 0; // Number of calibrated gyroscope
        static int nomagne = 0; // Number of calibrated magnetometer
        static MainpageCallback callback; // The Callback function
        static List<Thread> calThreads; // List of Calibration threads

        // Constants
        const int CALTIME = 10000; // Calibrating time

        #region Calibration

        // Calibrate the position (called at the Mainpage)
        public static void Calibrate(MainpageCallback callback)
        {
            Position.callback = callback;

            Normal = new Vector3(0, 0, 1); // Local vector will be calibrated later
            LocalX = new Vector3(1, 0, 0);
            Coordinates = new Vector3(0, 0, 0); // Initial coordinates

            nogyro = noacce = nomagne = 0;

            // Add sensors
            presensors = new List<ISensor>();
            presensors.Add(new InternalAccelerometer(SensorCallback, 0));
            sensors = new List<ISensor>();

            // Start calibration process
            calThreads = new List<Thread>(); // List of calibrating threads
            for (int i = 0; i < presensors.Count; i++)
            {
                Thread t =new Thread(new ParameterizedThreadStart(presensors[i].Calibrate));
                t.Start(CALTIME);
                calThreads.Add(t);
            }

            Thread preThread = new Thread(new ThreadStart(PreparationProcess));
            preThread.Start();
        }

        // Sensor calibration callback, use to send back the data
        public static void SensorCallback(int id, bool success, float[] data)
        {
            if (success)
            {
                // Accelerometer callback
                if (presensors[id].Type == SensorType.Accelerometer)
                {
                    noacce++;
                    gravity += new Vector3(data[0], data[1], data[2]);

                    // Add the sensor to the list
                    sensors.Add(presensors[id]);
                }

                // Gyroscope callback
                if (presensors[id].Type == SensorType.Gyroscope)
                {
                    // Add the sensor to the top the list
                    if (sensors.Count == 0 || sensors[0].Type != SensorType.Gyroscope)
                    {
                        sensors.Insert(0, presensors[id]);
                        nogyro++;
                    }
                }

                // Magnetometer callback
                if (presensors[id].Type == SensorType.Magnetometer)
                {
                    nomagne++;
                    magnetic += new Vector3(data[0], data[1], data[2]);

                    // Add the sensor to the list
                    sensors.Add(presensors[id]);
                }
            }

            // End the calibration thread
        }

        // Preparation thread process, process after all calibration threads are finished
        static void PreparationProcess()
        {
            // Wait until all threads are finished
            foreach (Thread t in calThreads)
                t.Join();

            // Preparation
            gravity /= noacce;
            magnetic /= nomagne;

            // Initialize the algorithm
            // Acceleration Separator algorithm for low-accuracy mode
            if (nogyro == 0)
                LowAccuracyModeInitialize();
            else
                HighAccuracyModeInitialize();

            // Init the watch
            Watch = Stopwatch.StartNew();
            nanosPerTick = (1000L * 1000L * 1000L) / Stopwatch.Frequency;

            // Callback to Mainpage
            callback(AccurateMode);
        }

        #endregion

        #region Mode Initialization

        // Low Accuracy Mode Initialize (called from PreparationProcess)
        // Return false if not enough sensors to operate
        static bool LowAccuracyModeInitialize()
        {
            AccurateMode = false;

            // Choose two sensors
            ASAccelerometer = null;
            ASMagnetometer = null;
            foreach (ISensor s in sensors)
            {
                if (ASAccelerometer == null && s.Type == SensorType.Accelerometer)
                    ASAccelerometer = s;

                if (ASMagnetometer == null && s.Type == SensorType.Magnetometer)
                    ASMagnetometer = s;

                if (ASAccelerometer != null && ASMagnetometer != null)
                    break;
            }

            // Check for adequacy
            if (ASAccelerometer == null || ASMagnetometer == null)
                return false;

            // Choose the update time
            if (ASAccelerometer.TimeBetweenUpdates > ASMagnetometer.TimeBetweenUpdates)
                ASTime = ASMagnetometer.TimeBetweenUpdates;
            else ASTime = ASAccelerometer.TimeBetweenUpdates;

            // Add update time to the list
            UpdateList = new List<UpdatePoint>();
            UpdateList.Add(new UpdatePoint(0, ASTime.Ticks * 100)); // Add the gyroscope to list

            // Init the algorithm
            AS = new S.AccelerationSeparator(magnetic, gravity, (float)ASTime.TotalSeconds);

            return true;
        }

        // High Accuracy Mode Initialize (called from PreparationProcess)
        // Return false if not enough sensors to operate
        static bool HighAccuracyModeInitialize()
        {
            // Init some variables
            AccurateMode = true;
            S.Matrix initNoise = new S.Matrix(3);

            // Init the algorithm
            EKL = new S.ExtendedKalmanFilter(3, nogyro*3, new float[]{0, 0, 0}, initNoise, AxisAngle, sensors[0].Noise);

            // Add update time to the list
            UpdateList = new List<UpdatePoint>();
            UpdateList.Add(new UpdatePoint(0, sensors[0].TimeBetweenUpdates.Ticks * 100)); // Add the gyroscope to list

            // Add observation models
            for (int i = 1; i < sensors.Count; i++)
            {
                int id = -1;
                if (sensors[i].Type == SensorType.Accelerometer)
                    id = EKL.AddObservationModel(Accelerometer, sensors[i].Noise);
                if (sensors[i].Type == SensorType.Magnetometer)
                    id = EKL.AddObservationModel(Magnetometer, sensors[i].Noise);
                if (id != i - 1) // ID of EKL is 0-based
                    throw new S.ExtendedKalmanFilterException("Add Observation: ID not match");
                // Add update time
                UpdateList.Add(new UpdatePoint(i, sensors[i].TimeBetweenUpdates.Ticks * 100)); // ID now is 1-based
            }

            UpdateList.Sort();

            return true;
        }

        #endregion

        #region Functions for Kalman Filter

        // State Transition
        public static float[] AxisAngle(float[] state, float[] control, long nanos)
        {
            if (state.Length != NOSTATES || control.Length != NOCONTROLS)
                throw new S.ExtendedKalmanFilterException("Axis Angle Transition: Invalid Length");

            // Old Quaternion (global)
            Vector3 AAState = new Vector3(state[0], state[1], state[2]);
            Quaternion QState = Quaternion.CreateFromAxisAngle(AAState, AAState.Length());

            // Transition (local)
            Vector3 AAControl = new Vector3(control[0], control[1], control[2]) * nanos / (float)1e9;
            Quaternion QControl = Quaternion.CreateFromAxisAngle(AAControl, AAControl.Length());

            // New Quaternion (Global)
            Quaternion QNewState = QState * QControl;
            
            // Get the axis
            Vector3 Axis = new Vector3(QNewState.X, QNewState.Y, QNewState.Z);
            Axis.Normalize();
            // Get the angle
            float Angle = (float)Math.Acos(QNewState.W);

            // Invert if the axis is opposite to the old one (when keeping old angle positive)
            if (Vector3.Dot(Axis, AAState) < 0)
            {
                Axis = -Axis;
                Angle = (float)Math.PI - Angle;
            }

            // Adjust the angle (close to the old angle)
            Angle *= 2;
            float diff = (Angle - AAState.Length()) % (float)(2 * Math.PI);
            if (diff > Math.PI) diff -= (float)(2 * Math.PI);
            if (diff < -Math.PI) diff += (float)(2 * Math.PI);
            Angle = AAState.Length() + diff;

            // New Axis Angle
            Axis *= Angle;

            return new float[3] { Axis.X, Axis.Y, Axis.Z };
        }

        // Accelerometer Model
        public static float[] Accelerometer(float[] state)
        {
            if (state.Length != NOSTATES)
                throw new S.ExtendedKalmanFilterException("Accelerometer Model: Invalid Length");

            // State Quaternion
            Vector3 AAState = new Vector3(state[0], state[1], state[2]);
            Quaternion QState = Quaternion.CreateFromAxisAngle(AAState, AAState.Length());

            Vector3 NewGravity = Vector3.Transform(gravity, QState);

            return new float[3] { NewGravity.X, NewGravity.Y, NewGravity.Z };
        }

        // Magnetometer Model
        public static float[] Magnetometer(float[] state)
        {
            if (state.Length != NOSTATES)
                throw new S.ExtendedKalmanFilterException("Magnetometer Model: Invalid Length");

            // State Quaternion
            Vector3 AAState = new Vector3(state[0], state[1], state[2]);
            Quaternion QState = Quaternion.CreateFromAxisAngle(AAState, AAState.Length());

            Vector3 NewMagnetic = Vector3.Transform(magnetic, QState);

            return new float[3] { NewMagnetic.X, NewMagnetic.Y, NewMagnetic.Z };
        }

        #endregion

        #region Mode Update Process

        // Update the position (start thread in Mainpage)
        public static void Update()
        {
            while (true)
            {
                if (UpdateList.Count > 0)
                {
                    long time = Watch.ElapsedTicks * nanosPerTick;
                    if (time > UpdateList[0].Time)
                    {
                        int CurrentID = UpdateList[0].ID;
                        if (AccurateMode) time = UpdateList[0].Time + sensors[CurrentID].TimeBetweenUpdates.Ticks * 100;
                        else time = UpdateList[0].Time + ASTime.Ticks * 100;

                        // Replace the old point by the new one
                        UpdateList.RemoveAt(0);
                        UpdateList.Add(new UpdatePoint(CurrentID, time));
                        UpdateList.Sort();

                        // Processes
                        if (AccurateMode) HighAccuracyModeProcess(CurrentID);
                        else LowAccuracyModeProcess();
                    }
                }
                Thread.Sleep(0);
            }
        }

        static void LowAccuracyModeProcess()
        {

        }

        static void HighAccuracyModeProcess(int id)
        {

        }

        #endregion

        public static void Stop()
        {
            stop = true;
            foreach (ISensor sensor in presensors)
                sensor.Stop();
        }
    }

    delegate void MainpageCallback(bool AccurateMode);

    struct UpdatePoint : IComparable
    {
        public int ID { get; private set; }
        public long Time { get; private set; }

        public UpdatePoint(int id, long time)
        {
            this.ID = id;
            this.Time = time;
        }

        public int CompareTo(object obj)
        {
            UpdatePoint compare = (UpdatePoint)obj;
            if (Time > compare.Time) return 1;
            else if (Time == compare.Time) return 0;
            else return -1;
        }
    }
}
